package nes;

import java.awt.Point;
import robocode.util.Utils;

/**
 * This is a Utility class contains some useful helper functions.
 * 
 * @author Sergey Negrashov
 * 
 */
public class PositionHelper {

  /**
   * Returns normalized absolute angle between two points.
   * 
   * @param origin origin point.
   * @param destination destination point.
   * @return normalized absolute angle.
   */
  public static double getAbsoluteAngle(Point origin, Point destination) {
    if (origin.equals(destination)) {
      return 0;
    }
    double dx = destination.getX() - origin.getX();
    double dy = destination.getY() - origin.getY();
    double turnAngle = Math.atan(dx / dy);
    // Tangent is only valid at -PI/2 to PI/2, we need to walk the quadrants and correct it.
    if (dx <= 0 && dy > 0) {
      turnAngle = Math.PI * 2 + turnAngle;
    }
    else if (dx > 0 && dy < 0) {
      turnAngle = Math.PI + turnAngle;
    }
    else if (dx <= 0 && dy < 0) {
      turnAngle = Math.PI + turnAngle;
    }
    return Utils.normalAbsoluteAngleDegrees(Math.toDegrees(turnAngle));
  }

  /**
   * Returns distance between two point.
   * 
   * @param origin origin point.
   * @param destination destination point.
   * @return distance between two points.
   */
  public static double getDistance(Point origin, Point destination) {
    double dx = origin.getX() - destination.getX();
    double dy = origin.getY() - destination.getY();
    return Math.sqrt(dx * dx + dy * dy);
  }

  /**
   * Computes a vector between two points.
   * 
   * @param origin point of origin.
   * @param destination destination.
   * @return angle normalized vector.
   */
  public static PolarVector getPolarVector(Point origin, Point destination) {
    return new PolarVector(getDistance(origin, destination), getAbsoluteAngle(origin, destination));
  }

  /**
   * Project vector onto a point. We are using robocode uses left-handed coordinate system so things
   * get loopy.
   * 
   * @param origin point of origin.
   * @param vec vector to project.
   * @return projected position.
   */
  public static Point projectPolarVector(Point origin, PolarVector vec) {
    // Math.sin(Math.toRadians(absBearingDeg)) * e.getDistance();
    double x = origin.getX() + vec.getR() * Math.sin(Math.toRadians(vec.getTheta()));
    double y = origin.getY() + vec.getR() * Math.cos(Math.toRadians(vec.getTheta()));
    Point ret = new Point();
    ret.setLocation(x, y);
    return ret;
  }

}
